#include "Serial.h"
#include <iostream>
//#include "Serial.h"

class OutputHandler
{
private:
	Serial* Arduino;
	char buffer[11];

public:
	OutputHandler()
	{
		Arduino = new Serial("COM5"); // Look in device manager to find the correct COM port
	}
	void setMode(int Mode)
	{
		//use fixed width buffer
		buffer[0] = '0';
		buffer[1] = Mode + 48; // convert to char
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}
	void SendPWM(float xPWM,float yPWM) // not yet implemented. waiting on motors 
	{

		sprintf(buffer,"2%1.2f,%1.2f0",xPWM,yPWM);
		printf(buffer);
		printf("\n");
		Arduino->WriteData(buffer,11);
	}
	void SendLaserOnOff(int state)
	{
		//use fixed width buffer
		buffer[0] = '1';
		buffer[1] = state + 48; // convert to char
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}
	void SendFire() // not yet implemented. waiting on motors
	{
		buffer[0] = '3';
		buffer[1] = '0'; 
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}

	int isConnected()
	{
		return Arduino->IsConnected();
	}

};


/*int main()
{
	 microcontrollerHandler * test;
	 int type;
	 int command;

	 test = new microcontrollerHandler();

	 while(!(test->isConnected()));

	 do
	 {
		 cout << "Enter 1 to change mode, 2 to change laser or 3 the exit" << endl;
		 
	     cin >> type;
		 if(type == 1)
		 {
			 cout << "Enter Mode (0-3)" << endl;
			 cin >> command;
			 test->setMode(command);
		 }
		 else if(type == 2)
		 {
			 cout << "Enter Laser status (0-1)" << endl;
			 cin >> command;
			 test->SendLaserOnOff(command);
		 }
	 }while(type != 3);

	 return 0; 
}*/